package app.util.algo;
import app.env.Data;
import app.env.Position;

import java.util.ArrayList;
import java.util.Collections;
import java.util.List;
import java.util.PriorityQueue;

public class JPS {
    protected PriorityQueue<Position> openList = new PriorityQueue<>();
    protected List<Position> opened = new ArrayList<>();
    protected List<Position> closed = new ArrayList<>();
    protected Position startPosition;
    protected Position endPosition;
    protected int[][] board;


    public JPS(int startX, int startY, int endX, int endY, int[][] board) {
        this.board = board;
        this.openList = new PriorityQueue<>();
        this.startPosition = new Position(startX, startY);
        this.endPosition = new Position(endX, endY);
        // push the start position into the open list
        this.openList.add(this.startPosition);
        opened.add(startPosition);
//        this.startPosition.setOpened(true);
    }

    public List<Position> findPath() {
        // while the open list is not empty
        while (!this.openList.isEmpty()) {
            // pop the position of position which has the minimum `f` value.
            Position position = this.openList.poll();
            closed.add(position);
//            position.setClosed(true);
            if (position.equals(endPosition)) {
                List<Position> backtrace = backtrace(position);
                List<Position> compress = compressPath(backtrace);
                List<Position> path = expandPath(compress);
                return path;
            }
            this.identifySuccessors(position);
        }
        // fail to find the path
        return null;
    }

    private void identifySuccessors(Position position) {
        int x = position.x;
        int y = position.y;
        List<Position> neighbors = this.findNeighbors(position);
        for (int i = 0, l = neighbors.size(); i < l; ++i) {
            Position neighbor = neighbors.get(i);
            Position jumpPoint = this.jump(neighbor.x, neighbor.y, x, y);
            if (jumpPoint != null) {
                int jx = jumpPoint.x;
                int jy = jumpPoint.y;

                if(closed.contains(jumpPoint)){
                    continue;
                }

                int d = octile(Math.abs(jx - x), Math.abs(jy - y));
                int ng = position.G + d; // next `g` value
                if (!opened.contains(jumpPoint) || ng < jumpPoint.G) {
                    jumpPoint.G = ng;
                    if (jumpPoint.H == 0) {
                        int endX = this.endPosition.x;
                        int endY = this.endPosition.y;
                        jumpPoint.H = heuristic(Math.abs(jx - endX), Math.abs(jy - endY));
                    }
                    jumpPoint.setFatherPos(position);
                    if (!opened.contains(jumpPoint)) {
                        this.openList.add(jumpPoint);
                        opened.add(jumpPoint);
//                        jumpPoint.setOpened(true);
                    }
                }
            }
        }
    }

    private List<Position> backtrace(Position position) {
        List<Position> paths = new ArrayList<Position>();
        paths.add(position);
        while (position.father != null) {
            position = position.father;
            paths.add(position);
        }
        Collections.reverse(paths);
        return paths;
    }

    private List<Position> compressPath(List<Position> path) {
        if (path.size() < 3) {
            return path;
        }
        List<Position> compressed = new ArrayList<>();
        int sx = path.get(0).x; // start x
        int sy = path.get(0).y; // start y
        int px = path.get(1).x; // second point x
        int py = path.get(1).y; // second point y
        int dx = px - sx; // direction between the two points
        int dy = py - sy; // direction between the two points
        int lx, ly;
        int ldx, ldy;
        int sq, i;
        sq = (int) Math.sqrt(dx * dx + dy * dy);
        dx /= sq;
        dy /= sq;
        compressed.add(new Position(sx, sy));
        for (i = 2; i < path.size(); i++) {
            lx = px;
            ly = py;
            ldx = dx;
            ldy = dy;
            px = path.get(i).x;
            py = path.get(i).y;
            dx = px - lx;
            dy = py - ly;
            sq = (int) Math.sqrt(dx * dx + dy * dy);
            dx /= sq;
            dy /= sq;
            if (dx != ldx || dy != ldy) {
                compressed.add(new Position(lx, ly));
            }
        }
        compressed.add(new Position(px, py));
        return compressed;
    }

    private List<Position> expandPath(List<Position> paths){

        List<Position> expand = new ArrayList<>();

        for (int i = 1; i < paths.size(); i++) {
            int previousX = paths.get(i - 1).x;
            int previousY = paths.get(i - 1).y;
            int currX = paths.get(i).x;
            int currY = paths.get(i).y;
            int dx = currX - previousX;
            int dy = currY - previousY;
            if (dx != 0 && dy != 0) {
                if (dx > 0 && dy < 0) {
                    while (currX != previousX && currY != previousY) {
                        expand.add(new Position(previousX,previousY));
                        previousX++;
                        previousY--;
                    }
                } else if (dx > 0 && dy > 0) {
                    while (currX != previousX && currY != previousY) {
                        expand.add(new Position(previousX,previousY));
                        previousX++;
                        previousY++;
                    }
                } else if (dx < 0 && dy > 0) {
                    while (currX != previousX && currY != previousY) {
                        expand.add(new Position(previousX,previousY));
                        previousX--;
                        previousY++;
                    }
                } else if (dx < 0 && dy < 0) {
                    while (currX != previousX && currY != previousY) {
                        expand.add(new Position(previousX,previousY));
                        previousX--;
                        previousY--;
                    }
                }
            }
            if (dx == 0) {
                if (dy < 0) {
                    while (currY != previousY) {
                        expand.add(new Position(previousX,previousY));
                        previousY--;
                    }
                } else {
                    while (currY != previousY) {
                        expand.add(new Position(previousX,previousY));
                        previousY++;
                    }
                }
            } else if (dy == 0) {
                if (dx < 0) {
                    while (currX != previousX) {
                        expand.add(new Position(previousX,previousY));
                        previousX--;
                    }
                } else {
                    while (currX != previousX) {
                        expand.add(new Position(previousX,previousY));
                        previousX++;
                    }
                }
            }
        }
        expand.add(paths.get(paths.size()-1));

        return expand;
    }
    
    public Position jump(int x, int y, int px, int py) {
        if (!isWalkableAt(x, y)) {
            return null;
        }
        Position position = new Position(x, y);
        if (position.equals(endPosition)) {
            return position;
        }
        int dx = x - px;
        int dy = y - py;
        // 查找强制邻居
        // 沿对角线方向移动
        if (dx != 0 && dy != 0) {
            // 当沿着对角线移动时，必须检查水平和垂直方向上的跳点
            if (this.jump(x + dx, y, x, y) != null || this.jump(x, y + dy, x, y) != null) {
                return position;
            }
        }
        // 水平和垂直方向
        else {
            if (dx != 0) {
                if ((isWalkableAt(x, y - 1) && !isWalkableAt(x - dx, y - 1))
                        || (isWalkableAt(x, y + 1) && !isWalkableAt(x - dx, y + 1))) {
                    return position;
                }
            } else if (dy != 0) {
                if ((isWalkableAt(x - 1, y) && !isWalkableAt(x - 1, y - dy))
                        || (isWalkableAt(x + 1, y) && !isWalkableAt(x + 1, y - dy))) {
                    return position;
                }
            }
        }
        // 对角线移动，必须确保其中一个垂直/水平
        // neighbors is open to allow the path
        if (isWalkableAt(x + dx, y) && isWalkableAt(x, y + dy)) {
            return this.jump(x + dx, y + dy, x, y);
        } else {
            return null;
        }
    }

    protected List<Position> findNeighbors(Position position) {
        List<Position> neighbors = new ArrayList<>();
        Position parent = position.father;
        // 直接剪枝：可以忽略大多数邻居，除非强制邻居
        if (parent != null) {
            int px = parent.x;
            int py = parent.y;
            int x = position.x;
            int y = position.y;
            // 得到方向
            int dx = (x - px) / Math.max(Math.abs(x - px), 1);
            int dy = (y - py) / Math.max(Math.abs(y - py), 1);
            // 对角方向搜索
            if (dx != 0 && dy != 0) {
                if (isWalkableAt(x, y + dy)) {
                    neighbors.add(new Position(x, y+dy));
                }
                if (isWalkableAt(x + dx, y)) {
                    neighbors.add(new Position(x+dx, y));
                }
                if (isWalkableAt(x, y + dy) && isWalkableAt(x + dx, y)) {
                    neighbors.add(new Position(x + dx, y + dy));
                }
            }
            // search horizontally/vertically
            else {
                boolean isNextWalkable;
                if (dx != 0) {
                    isNextWalkable = isWalkableAt(x + dx, y);
                    boolean isTopWalkable = isWalkableAt(x, y + 1);
                    boolean isBottomWalkable = isWalkableAt(x, y - 1);
                    if (isNextWalkable) {
                        neighbors.add(new Position( x + dx, y));
                        if (isTopWalkable) {
                            neighbors.add(new Position(x + dx, y + 1));
                        }
                        if (isBottomWalkable) {
                            neighbors.add(new Position(x + dx, y - 1));
                        }
                    }
                    if (isTopWalkable) {
                        neighbors.add(new Position(x, y + 1));
                    }
                    if (isBottomWalkable) {
                        neighbors.add(new Position(x, y - 1));
                    }
                } else if (dy != 0) {
                    isNextWalkable = isWalkableAt(x, y + dy);
                    boolean isRightWalkable = isWalkableAt(x + 1, y);
                    boolean isLeftWalkable = isWalkableAt(x - 1, y);
                    if (isNextWalkable) {
                        neighbors.add(new Position(x, y + dy));
                        if (isRightWalkable) {
                            neighbors.add(new Position(x + 1, y + dy));
                        }
                        if (isLeftWalkable) {
                            neighbors.add(new Position(x - 1, y + dy));
                        }
                    }
                    if (isRightWalkable) {
                        neighbors.add(new Position(x + 1, y));
                    }
                    if (isLeftWalkable) {
                        neighbors.add(new Position(x - 1, y));
                    }
                }
            }
        }
        // return all neighbors
        else {
            List<Position> neighborPositions = getNeighbors(position);
            for (int i = 0, l = neighborPositions.size(); i < l; ++i) {
                Position neighborPosition = neighborPositions.get(i);
                neighbors.add(neighborPosition);
            }
        }
        return neighbors;
    }
    public List<Position> getNeighbors(Position position) {
        int x = position.x;
        int y = position.y;
        boolean s0 = false;
        boolean s1 = false;
        boolean s2 = false;
        boolean s3 = false;
        List<Position> neighbors = new ArrayList<Position>();
        // ↑
        if (this.isWalkableAt(x, y - 1)) {
            neighbors.add(new Position(x, y - 1));
//            neighbors.add(new Position(y-1, x));
            s0 = true;
        }
        // →
        if (this.isWalkableAt(x + 1, y)) {
            neighbors.add(new Position(x + 1, y));
//            neighbors.add(new Position(y, x+1));
            s1 = true;
        }
        // ↓
        if (this.isWalkableAt(x, y + 1)) {
            neighbors.add(new Position(x, y + 1));
//            neighbors.add(new Position(y+1, x));
            s2 = true;
        }
        // ←
        if (this.isWalkableAt(x - 1, y)) {
            neighbors.add(new Position(x - 1, y));
//            neighbors.add(new Position(y, x - 1));
            s3 = true;
        }
        boolean d0 = s3 && s0;
        boolean d1 = s0 && s1;
        boolean d2 = s1 && s2;
        boolean d3 = s2 && s3;
        // ↖
        if (d0 && this.isWalkableAt(x - 1, y - 1)) {
            neighbors.add(new Position(x - 1, y - 1));
//            neighbors.add(new Position(y - 1, x - 1));
        }
        // ↗
        if (d1 && this.isWalkableAt(x + 1, y - 1)) {
            neighbors.add(new Position(x + 1, y - 1));
//            neighbors.add(new Position(y - 1, x + 1));
        }
        // ↘
        if (d2 && this.isWalkableAt(x + 1, y + 1)) {
            neighbors.add(new Position(x + 1, y + 1));
//            neighbors.add(new Position(y + 1, x + 1));
        }
        // ↙
        if (d3 && this.isWalkableAt(x - 1, y + 1)) {
            neighbors.add(new Position(x - 1, y + 1));
//            neighbors.add(new Position(y + 1, x - 1));
        }
        return neighbors;
    }
    private boolean isWalkableAt(int x, int y){
        if(x <0 || x >= board.length ||
                y < 0 || y >= board[0].length ||
                this.board[x][y] == Data.OBSTACLE_SIGNAL||
                this.board[x][y] == Data.TARGET_SIGNAL)
            return false;
        else
            return true;
    }
    public int octile(int dx, int dy) {
        return (int) (Math.max(dx, dy) + (Math.sqrt(2) - 1) * Math.min(dx, dy));
    }

    public int heuristic(int dx, int dy) {
        return (int) Math.sqrt(dx * dx + dy * dy);
    }
}
